#ifndef KEYFRAME_H
#define KEYFRAME_H
#include "common.h"
#include "MapPoint.h"
#include "Frame.h"
#include "Map.h"

class MapPoint;
class Frame;
class Map;

class KeyFrame
{
public:
	KeyFrame(Frame &F, Map *pMap);
	~KeyFrame();

	cv::Mat GetCameraCenter();

	void SetPose(const cv::Mat &RTcw);

	cv::Mat GetPose();

	cv::Mat GetPoseInverse();

	cv::Mat GetStereoCenter();

	cv::Mat GetRotation();

	cv::Mat GetTranslation();

	// Covisibility graph functions
	void AddConnection(KeyFrame* pKF, const int &weight);

	void EraseConnection(KeyFrame* pKF);

	void UpdateConnections();

	void UpdateBestCovisibles();

	std::set<KeyFrame *> GetConnectedKeyFrames();

	std::vector<KeyFrame* > GetVectorCovisibleKeyFrames();

	std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int &N);

	std::vector<KeyFrame*> GetCovisiblesByWeight(const int &w);

	int GetWeight(KeyFrame* pKF);


	static bool weightComp( int a, int b){ return a>b;}

	// Spanning tree functions
	void AddChild(KeyFrame* pKF);

	void EraseChild(KeyFrame* pKF);

	void ChangeParent(KeyFrame* pKF);

	std::set<KeyFrame*> GetChilds();

	KeyFrame* GetParent();

	bool hasChild(KeyFrame* pKF);

	// Loop Edges
	void AddLoopEdge(KeyFrame* pKF);

	std::set<KeyFrame*> GetLoopEdges();


	// MapPoint observation functions
	void AddMapPoint(MapPoint* pMP, const size_t &idx);

	void EraseMapPointMatch(const size_t &idx);

	void EraseMapPointMatch(MapPoint* pMP);

	void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP);

	std::set<MapPoint*> GetMapPoints();

	std::vector<MapPoint*> GetMapPointMatches();

	int TrackedMapPoints(const int &minObs);

	MapPoint* GetMapPoint(const size_t &idx);


	// KeyPoint functions
	cv::Mat UnprojectStereo(int i);

	void SetNotErase();

	void SetErase();

	void SetBadFlag();

	bool isBadKeyFrame();

	bool IsInImage(const float &x, const float &y);

	std::vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r);

public:

	// Number of KeyPoints
    const int m_keyPointsNumber;

	long unsigned int m_KeyFrameId;

	const long unsigned int m_frameId;

	// Calibration parameters
    const float m_fx;
	const float m_fy;
	const float m_cx;
	const float m_cy;
	const float m_invfx;
	const float m_invfy;
	const float m_baselineFx;
	const float m_baseline;

	float m_ThDepth;

	// Pose relative to parent (this is computed when bad flag is activated)
    cv::Mat m_RTcp;
	


	// KeyPoints, stereo coordinate and descriptors (all associated by an index)
	const std::vector<cv::KeyPoint> m_vectorKeyPoints;
	const std::vector<cv::KeyPoint> m_vectorKeyPointsUn;
	const std::vector<float> m_vectorURight;
	const std::vector<float> m_vectorDepth;
	const cv::Mat m_Descriptors;


	const cv::Mat m_K;

	// Variables used by the tracking
	long unsigned int m_TrackReferenceForFrame;
	long unsigned int m_FuseTargetForKF;

	// Variables used by the local mapping
	long unsigned int m_BALocalForKF;
	long unsigned int m_BAFixedForKF;

	float m_MinX;
	float m_MaxX;
	float m_MinY;
	float m_MaxY;

	// Grid (to speed up feature matching)
    int m_GridCols;
    int m_GridRows;
    float m_GridElementWidthInv;
    float m_GridElementHeightInv;

	// Grid over the image to speed up feature matching
    std::vector< std::vector <std::vector<size_t> > > m_Grid;

	Vector3f m_position;

	//x is roll, y is pitch, z yaw
	Vector3f m_attitude;

protected:

	// SE3 Pose and camera center

	//camera to world rotation and translation
    cv::Mat m_RTcw;

	//world to camera rotation and translation
    cv::Mat m_RTwc;

	//camera center
    cv::Mat m_Ow;

	// Stereo middel point. Only for visualization
	cv::Mat m_Cw;

	float m_HalfBaseline;

	// MapPoints associated to keypoints
	std::vector<MapPoint*> m_vectorMapPoints;

	// Bad flags
    bool m_NotErase;
	bool m_ToBeErased;
	bool m_BadKeyFrame;

	std::map<KeyFrame*,int> m_ConnectedKeyFrameWeights;
	std::vector<KeyFrame*> m_vectorOrderedConnectedKeyFrames;
	std::vector<int> m_vectorOrderedWeights;

	// Spanning Tree and Loop Edges
	bool m_FirstConnection;
	KeyFrame* m_Parent;
	std::set<KeyFrame*> m_setChildrens;
	std::set<KeyFrame*> m_setLoopEdges;


	Map* m_Map;


	std::mutex m_MutexPose;
	std::mutex m_MutexConnections;
	std::mutex m_MutexFeatures;
};


#endif //KEYFRAME_H
